#include "Code3_DEBUG.h"
#include "Code1_MOTOR.h"	
#include "Code0_CONF_MOTION.h"


// USART Buffer for Zigbee
unsigned char ZigbeeRX[ZIGBEE_RX_NUM] = {0};
unsigned char ZigbeeTX[ZIGBEE_TX_NUM] = {0};

unsigned char Zigbee_SendBackCheck[5] = {0xAA, 0xAA, 0xAA, 0xAA, 0xAA};
Bool Zigbee_Disabled = FALSE;
Bool Zigbee_SendMotorData = FALSE;
Bool Zigbee_Ready2Disable = FALSE;
													 
void ZigbeeRX_Init(void)
{
	int i;

	for(i=0; i < ZIGBEE_RX_NUM; i++)
	{
		ZigbeeRX[i] = 0;
	}
}

PacketStatus ZigbeeAccessRX(void)
{														
	#ifdef DEBUG_PORT			 
	int i = 0;
	unsigned char check_sum = 0;

	for(i=0; i < (ZIGBEE_RX_NUM-1); i++)
	{
		check_sum = check_sum + ZigbeeRX[i]; 		
	}
	check_sum = ~check_sum;

	// if checksum is wrong, return	PacketStatus = WRONG
	if(ZigbeeRX[(ZIGBEE_RX_NUM-1)] != check_sum)
	{
		return WRONG;
	}
	
	if((ZigbeeRX[0] == 0xAA) && (ZigbeeRX[1] == 0xAA) && (ZigbeeRX[2] == 0xAA) &&
		 (ZigbeeRX[3] == 0xAA) && (ZigbeeRX[4] == 0xAA)) 
	{
		if(Zigbee_SendMotorData == FALSE)
		{																																							 
			Zigbee_SendMotorData = TRUE;
			CalculateReadChecksum();
			DMA2_Configuration(&(MotorData.Read[0][0]), (MOTOR_READ_TOTAL_NUM * MOTOR_READ_LENGTH));	
		}
	}							
	else if(ZigbeeRX[INSTRUCTION_MODE] >= DEBUG_INST_BOUND || ZigbeeRX[INSTRUCTION_MODE] < SET_TORQUE) 
	{
		// Received correct data, send signal to HMI
//		if(!((ZigbeeRX[INSTRUCTION_MODE] <= RETURN_MOTOR_READ) && 
//				(ZigbeeRX[INSTRUCTION_MODE] >= DEBUG_INST_BOUND)))
//		{
			for(i=0; i < sizeof(Zigbee_SendBackCheck); i++)
			{
				USART_SendData(DEBUG_PORT, Zigbee_SendBackCheck[i]);    
				while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
				{
				}
			}				
//		}

		if(ZigbeeRX[INSTRUCTION_MODE] == SCHEDULE_SWITCH)
		{
			MotorScheduleSwitch(ZigbeeRX[1]);

			if(MotorSchedule.Enabled == FALSE)
			{
				MotorSchedule.NowIndex = 0;
				MotorSchedule.EndIndex = 0;
				MotorSchedule.Delay_ExecutedCount = 0;
				Zigbee_Ready2Disable = TRUE;
			}
			else
			{
				if(Zigbee_Ready2Disable == TRUE)
				{
					USART_ITConfig(DEBUG_PORT, USART_IT_RXNE, DISABLE);
					USART_ClearFlag(DEBUG_PORT, USART_FLAG_ORE);
					USART_ClearITPendingBit(DEBUG_PORT, USART_IT_RXNE);
				}
			}
		}			
		else if(ZigbeeRX[INSTRUCTION_MODE] == REMOTE_HEAD_MODE)
		{		
			Head.Mode_RX = ZigbeeRX[1];
		}
		else if(ZigbeeRX[INSTRUCTION_MODE] == REMOTE_HEAD_POSITION)
		{				 			
      Add_head_job(0, ZigbeeRX[1], ZigbeeRX[2], 0, 1);
      Add_head_job(1, ZigbeeRX[3], ZigbeeRX[4], 0, 1);
		}
		else if(ZigbeeRX[INSTRUCTION_MODE] == REMOTE_MOITON)
		{																			
			MotorScheduleSwitch(ON);
			Motion.Mode_RX = ZigbeeRX[1];
			
			if(Motion.Mode_RX == WALK)
			{
				if(ZigbeeRX[2] == 1) 
				{  // Walk_F
					Motion.ContinueDistance_RX = (ZigbeeRX[3]) * WALK_F_DISTANCE;
				} 
				else if(ZigbeeRX[2] == 2) 
				{  // Walk_FR
					Motion.ContinueDistance_RX = (ZigbeeRX[3]) * WALK_FR_DISTANCE; 
					Motion.ContinueAngle_RX = ZigbeeRX[4] * WALK_FR_ANGLE; 
				}
				else if(ZigbeeRX[2] == 3) 
				{  // Walk_FL
					Motion.ContinueDistance_RX = (ZigbeeRX[3]) * WALK_FL_DISTANCE; 
					Motion.ContinueAngle_RX = ZigbeeRX[4] * WALK_FL_ANGLE; 
				}
				else if(ZigbeeRX[2] == 4) 
				{  // Walk_B
					Motion.ContinueDistance_RX = (ZigbeeRX[3]) * WALK_B_DISTANCE; 
				}
				else if(ZigbeeRX[2] == 5) 
				{  // Walk_BR
					Motion.ContinueDistance_RX = (ZigbeeRX[3]) * WALK_BR_DISTANCE; 
					Motion.ContinueAngle_RX = ZigbeeRX[4] * WALK_BR_ANGLE; 
				}
				else if(ZigbeeRX[2] == 6) 
				{  // Walk_BL
					Motion.ContinueDistance_RX = (ZigbeeRX[3]) * WALK_BL_DISTANCE; 
					Motion.ContinueAngle_RX = ZigbeeRX[4] * WALK_BL_ANGLE; 
				}
				else {													
					Motion.ContinueDistance_RX = 0;
				 	Motion.ContinueAngle_RX = 0;
				}
			}
			else if(Motion.Mode_RX == SHIFT)
			{
				Motion.ContinueAngle_RX = 0;
				if(ZigbeeRX[2] == 1)
					Motion.ContinueDistance_RX = ZigbeeRX[4] * SHIFT_R_DISTANCE;
				else if(ZigbeeRX[2] == 2)
					Motion.ContinueDistance_RX = ZigbeeRX[4] * SHIFT_L_DISTANCE;
				else
					Motion.ContinueDistance_RX = 0;
			}
			else if(Motion.Mode_RX == TURN)
			{									 
				Motion.ContinueDistance_RX = 0;
				if(ZigbeeRX[2] == 1)
					Motion.ContinueAngle_RX = ZigbeeRX[4] * TURN_R_ANGLE;
				else if(ZigbeeRX[2] == 2)
					Motion.ContinueAngle_RX = ZigbeeRX[4] * TURN_L_ANGLE;
				else
					Motion.ContinueAngle_RX = 0; 				
			}
			else if(Motion.Mode_RX == TURNBALL)
			{
				Motion.ContinueDistance_RX = 0;
				if(ZigbeeRX[2] == 1)
					Motion.ContinueAngle_RX = ZigbeeRX[4] * TURNBALL_R_ANGLE;
				else if(ZigbeeRX[2] == 2)
					Motion.ContinueAngle_RX = ZigbeeRX[4] * TURNBALL_L_ANGLE;
				else
					Motion.ContinueAngle_RX = 0; 			
			}
			#ifdef THROW  										 
			else if(Motion.Mode_RX == THROW)			 
			{																	 
				if(ZigbeeRX[2] == 0)	
				{
					Motion.ContinueDistance_RX = 145;
				}
				else
				{
					Motion.ContinueDistance_RX = ZigbeeRX[2];
				}													
				Motion.ContinueAngle_RX = 0;
			}
			#endif
			#ifdef ROTATE_WAIST
			else if(Motion.Mode_RX == ROTATE_WAIST)	
			{
				if(ZigbeeRX[2] == 0)								
				{
					Motion.ContinueAngle_RX = ZigbeeRX[3];
				}
				else if(ZigbeeRX[2] == 1)
				{																				
					Motion.ContinueAngle_RX = -ZigbeeRX[3];
				}
				else
				{																			 
					Motion.ContinueAngle_RX = 0;
				}
				Motion.ContinueDistance_RX = 0;
			}
			#endif
			else
			{
				Motion.ContinueAngle_RX = 0;
				Motion.ContinueDistance_RX = 0;
			}
		} // End: else if(ZigbeeRX[INSTRUCTION_MODE] == REMOTE_MOITON)
		else if(ZigbeeRX[INSTRUCTION_MODE] == RETURN_MOTOR_READ)
		{
			int id = ZigbeeRX[1];
			int i;									
			if((id >= 0) && (id < MOTOR_NUM))
			{					
				for(i=0; i < MOTOR_READ_LENGTH; i++)
				{	
					USART_SendData(DEBUG_PORT, MotorData.Read[id][i]);
					while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TC) == RESET)
					{											
					}
				}	
			}
		}
		else if(ZigbeeRX[INSTRUCTION_MODE] == GET_DMA_FLAG)
		{					
			USART_SendData(DEBUG_PORT, DMA_GetFlagStatus(DMA1_FLAG_TC4));
			while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TC) == RESET)
			{
			}																												
			USART_SendData(DEBUG_PORT, DMA_GetFlagStatus(DMA1_FLAG_TC2));
			while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TC) == RESET)
			{
			}					
			USART_SendData(DEBUG_PORT, DMA_GetFlagStatus(DMA1_FLAG_TC7));
			while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TC) == RESET)
			{
			}					
		}
		else if(ZigbeeRX[INSTRUCTION_MODE] == GET_MOTOR_DATA)
		{
			int id, part, no;

			id = ZigbeeRX[1];
			IDtoPartNo(id, &part, &no);					
			USART_SendData(DEBUG_PORT, id);
			while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TC) == RESET)
			{
			}					
			USART_SendData(DEBUG_PORT, *(MotorAddress.Write[part] + (no * MOTOR_WRITE_LENGTH) + LOW_POSITION));
			while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TC) == RESET)
			{
			}			
			
			USART_SendData(DEBUG_PORT, *(MotorAddress.Write[part] + (no * MOTOR_WRITE_LENGTH) + HIGH_POSITION));
			while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TC) == RESET)
			{
			}			
		}	
		else if(ZigbeeRX[INSTRUCTION_MODE] == GET_SCHEDULE_INDEX)
		{																						 
			USART_SendData(DEBUG_PORT, MotorSchedule.NowIndex);
			while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TC) == RESET)
			{
			}					
			USART_SendData(DEBUG_PORT, MotorSchedule.EndIndex);
			while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TC) == RESET)
			{
			}			
		}
		else if(ZigbeeRX[INSTRUCTION_MODE] == GET_SCHEDULE_DATA)
		{
			unsigned char temp_now = (ZigbeeRX[1] >= MOTOR_SCHEDULE_SIZE) ? (MOTOR_SCHEDULE_SIZE-1) : ZigbeeRX[1];
			unsigned char temp_end = (ZigbeeRX[2] >= MOTOR_SCHEDULE_SIZE) ? (MOTOR_SCHEDULE_SIZE-1) : ZigbeeRX[2];
			int i, j;

			for(i=temp_now; i < temp_end; i++)
			{
				for(j=0; j < MOTOR_SCHEDULE_LIST_LENGTH; j++)
				{			 					
					USART_SendData(DEBUG_PORT, MotorSchedule.List[i][j]);
					while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TC) == RESET)
					{
					}			
				}
			}
		}
		else //if(ZigbeeRX[INSTRUCTION_MODE] < SET_TORQUE) 
		{
			if((MotorSchedule.Enabled == TRUE) ||
				 ((MotorSchedule.EndIndex > 0) && 
				  (MotorSchedule.List[MotorSchedule.EndIndex-1][0] != ZigbeeRX[0])) ||
				 ((MotorSchedule.EndIndex == 0) && 
				  (MotorSchedule.List[MOTOR_SCHEDULE_SIZE-1][0] != ZigbeeRX[0])))
			{
//				if(MotorSchedule.Capacity != FULL)
//				{
					for(i=0; i<5; i++) 
					{
						MotorSchedule.List[MotorSchedule.EndIndex][i] =  ZigbeeRX[i];
					}
					MotorSchedule.EndIndex = (MotorSchedule.EndIndex + 1) % MOTOR_SCHEDULE_SIZE;
	
					if((MotorSchedule.NowIndex == 0) && (MotorSchedule.EndIndex == (MOTOR_SCHEDULE_SIZE-1)))
					{
						MotorSchedule.Capacity = FULL;
					}
					else if((MotorSchedule.EndIndex - MotorSchedule.NowIndex) == 1)
					{
						MotorSchedule.Capacity = FULL;
					}
					else
					{
						MotorSchedule.Capacity = NONEMPTY;
					}
//				}
			}
		}
	}		 

	return CORRECT;
	#endif
}

